/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
#include "fusion_component.h"
#include "utils/base/object_pool_types.h"
#include "utils/common/perception_gflags.h"

namespace fusion {

uint32_t FusionComponent::s_seq_num_ = 0;
std::mutex FusionComponent::s_mutex_;

bool FusionComponent::Init() {
    // proto/fusion_component_config.proto
    FusionComponentConfig comp_config;
    if (!common::GetProtoFromFile(FLAGS_multi_sensor_fusion_conf_path, &comp_config)) {
        std::cout << "ERROR: "
                  << "Init: failed to load config file." << std::endl;
        return false;
    }

    std::cout << "Fusion Component Configs: " << comp_config.DebugString() << std::endl;

    // to load component configs
    fusion_name_ = comp_config.fusion_name();
    fusion_method_ = comp_config.fusion_method();
    fusion_main_sensor_ = comp_config.fusion_main_sensor();
    object_in_roi_check_ = comp_config.object_in_roi_check();
    radius_for_roi_object_check_ = comp_config.radius_for_roi_object_check();

    // init algorithm plugin
    if (!InitAlgorithmPlugin()) {
        std::cout << "Failed to init algorithm plugin." << std::endl;
    }
    return true;
}

bool FusionComponent::Proc() {
    // 接收数据message
    // 解析数据[目标状态信息、时间戳、sensor_name、sensor2world_pose]
    // 遍历obj size
    base::FramePtr frame(new base::Frame());
    // sensor_info type + name
    // object -> fusion_object
    // frame.empalce_back(fusion_object)
    std::vector<base::ObjectPtr> fused_objects;
    // 调用process
    if (!fusion_->Process(frame, &fused_objects)) {
        std::cout << "ERROR: "
                  << "Failed to call fusion plugin." << std::endl;
        return false;
    }
    // 发布valid_objects
}

bool FusionComponent::InitAlgorithmPlugin() {
    // @liuxinyu: ObstacleMultiSensorFusion 融合具体实现方法
    fusion::BaseMultiSensorFusion* fusion =
        fusion::BaseMultiSensorFusionRegisterer::GetInstanceByName(fusion_name_);
    if (fusion == nullptr) {
        return false;
    }
    fusion_.reset(fusion);
    // fusion_.reset(new fusion::ObstacleMultiSensorFusion());
    fusion::ObstacleMultiSensorFusionParam param;
    param.main_sensor = fusion_main_sensor_;
    param.fusion_method = fusion_method_;
    if (!fusion_->Init(param))
        std::cout << "Failed to init ObstacleMultiSensorFusion" << std::endl;

    std::cout << "Init algorithm successfully, onboard fusion: " << fusion_method_ << std::endl;
    return true;
}

}  // namespace fusion
